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Abstract —State-of-the-art techniques for simultaneous local¬ 
ization and mapping (SLAM) employ iterative nonlinear opti¬ 
mization methods to compute an estimate for robot poses. While 
these techniques often work well in practice, they do not provide 
guarantees on the quality of the estimate. This paper shows that 
Lagrangian duality is a powerful tool to assess the quality of a 
given candidate solution. Our contribution is threefold. First, we 
discuss a revised formulation of the SLAM inference problem. 
We show that this formulation is probabilistically grounded and 
has the advantage of leading to an optimization problem with 
quadratic objective. The second contribution is the derivation 
of the corresponding Lagrangian dual problem. The SLAM 
dual problem is a (convex) semidefinite program, which can be 
solved reliably and globally by off-the-shelf solvers. The third 
contribution is to discuss the relation between the original SLAM 
problem and its dual. We show that from the dual problem, 
one can evaluate the quality (i.e., the suboptimality gap) of a 
candidate SLAM solution, and ultimately provide a certificate 
of optimality. Moreover, when the duality gap is zero, one can 
compute a guaranteed optimal SLAM solution from the dual 
problem, circumventing non-convex optimization. We present 
extensive (real and simulated) experiments supporting our claims 
and discuss practical relevance and open problems. 


1. Introduction 

Simultaneous localization and mapping (SLAM) is an en¬ 
abling technology for many applications, including service and 
industrial robotics, autonomous driving, search and rescue, 
planetary exploration, and augmented reality. 

The last decade has witnessed several groundbreaking re¬ 
sults in SLAM, and state-of-the-art approaches are now tran¬ 
sitioning from academic research to industrial applications. 
Standard techniques compute an estimate (e.g., for robot 
poses) by minimizing a nonlinear cost function, whose global 
minimum is the maximum likelihood estimate (or maximum 
a posteriori estimate in presence of priors). The optimization 
problem underlying SLAM is commonly solved using itera¬ 
tive nonlinear optimization methods, e.g. the Gauss-Newton 
method Q, O, O, the gradient method IH, O, trust region 
methods O, or ad-hoc approximations fTll. ifSl. 

Despite the success of state-of-the-art techniques, some 
practical and theoretical problems remain open. While iterative 
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Fig. 1. This paper exploits Lagrangian duality to evaluate the quality 
of a SLAM solution. For instance, our verification techniques are able to 
certify the optimality of the solutions in the first row of the figure, while 
they reject as suboptimal the estimates shown in the bottom row. 


approaches are observed to work well in many problem in¬ 
stances, they cannot guarantee the correctness (global optimal¬ 
ity) of the estimates that they compute. This is due to the fact 
that the optimization problem is non-convex, hence iterative 
techniques may be trapped in local minimc[^ (Fig.^, which 
correspond to wrong estimates. Recent work 0, nol shows 
that iterative techniques fail to converge to a correct estimate 
even in fairly simple (real and simulated) 3D problems. Recent 
research efforts have addressed the issue of global convergence 
from several angles. Olson et al (H, Grisetti et al O, 
Rosen et al |6], and Tron et al GD Study iterative techniques 
with larger basins of convergence. Carlone et al 0, HD, and 
Rosen et al cni propose initialization techniques to bootstrap 
iterative optimization. Huang et al lfT3]| . Wang et al iMl, 
Carlone ca, and Khosoussi et al investigate the factors 
infiuencing local convergence and the quality of the optimal 
SLAM solution. While these techniques provide remarkable 
insights into the problem, and working solutions to improve 
convergence, none of them can guarantee the recovery of a 
globally optimal solution to SLAM. 

The motivation behind this work is that the transition of 
SLAM from research topic to industrial technology requires 
techniques with guaranteed performance. In autonomous ve¬ 
hicle applications, failure to produce a correct SLAM solution 
may put passengers’ lives at risk. In other applications, SLAM 
failures can possibly cascade into path planning failures (if the 
plan is computed using a wrong map), and this may prevent 
the reliable operation of mobile robots. 

Therefore, in this paper we address the following question: 
Verification Problem: Given a candidate SLAM estimate 
(e.g., returned by a state-of-the-art iterative solver), is it 


^We use the term “local minimum” to denote a stationary point of the cost 
which does not attain the optimal objective. 
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possible to evaluate the quality of this estimate (e.g., its sub¬ 
optimality gap), possibly certifying its optimality? 

To address this problem, we introduce a powerful tool, 
Lagrangian duality, borrowing the corresponding theory from 
the optimization community. Duality was first applied to 2D 
SLAM by Carlone and Dellaert (91. In this work, we provide 
a nontrivial extension of (3 to 3D SLAM. 

This paper contains three main contributions. The first 
contribution is a revised formulation of the SLAM inference 
problem. Our formulation is a probabilistically grounded max¬ 
imum likelihood (ML) estimator, and has the advantage of 
leading to an optimization problem with quadratic objective, 
which facilitates the derivation of the dual problem. Our 
revised SLAM formulation is presented in Section 

The second contribution is the derivation of the Lagrangian 
dual problem. A key step towards this goal consists of 
rewriting the 3D SLAM problem as a quadratic optimization 
problem with quadratic equality constraints. Intuitively, the 
constraints impose that the pose estimates are members of 
SE(3), while the objective minimizes the mismatch w.r.t. 
the measurements. The dual SLAM problem, introduced in 
Section [II^ is a (convex) semidefinite program (SDP), and 
can be solved globally by off-the-shelf solvers. 

The third contribution is to provide verification techniques 
to assess the quality of a given SLAM solution, leveraging the 
relation between the standard SLAM problem and its dual. 
We show that solving the dual problem allows us to bound 
the sub-optimality gap of a given candidate solution, hence 
we are able to quantify how far the candidate solution is 
from being optimal. Since current SDP solvers do not scale 
well to large problems, we also propose a second verification 
technique that does not require solving the SDP. As a by¬ 
product of our derivation, we show that, when the duality gap 
is zero, we can compute an optimal SLAM solution directly 
from the dual problem. Our verification techniques, presented 
in Section |IVj can be seamlessly integrated in standard SLAM 
pipelines. Experimental evidence (Section 0 confirms that 
these techniques enable the certification of globally optimal 
solutions in both real and simulated experiments. Extra results 
and visualizations are given in the appendix of this paper. 


11. 3D Pose Graph Optimization Revisited 


We consider the pose graph optimization (PGO) formu¬ 
lation of the SLAM problem. PGO computes the maximum 
likelihood estimate for n poses cci,..., ccn, given m relative 
pose measurements Xij between pairs of poses i and j. In a 
3D setup, both the unknown poses and the measurements are 
quantities in SE(3) = {{R,t) : R G SO(3),t G M^}. We 
use the notation Xi = {Ri^ti) and Xij = {Rij^tij) to make 
explicit the rotation and the translation of each pose. PGO 
can be visualized as a directed graph Q{V,S), in which we 
associate a node i G V = {!,.. .,n} to each pose Xi and an 
edge {i,j)eS to each relative measurement Xij. 


In this section we propose a revised PGO formulation. 
The key difference w.r.t. related work is the use of the 
chordal distance to quantify the rotation errors (more details 
in Section [ILB]). To lay the groundwork for this formulation. 


we begin with a generative model for our measurements, and 
then derive the corresponding ML estimator. 

A. Generative Noise Model 

We assume the following ^nerative model for the relative 
pose measurements {Rij^iij^ 

Uj = te ~ gaussian(03,W(2l3) 

Rij = RjRjRf^ ~'vonMises(l3,a;^) 

where “gaussian(/L4, fi)” denotes a Gaussian distribution with 
mean p, and information matrix Q,, while “vonMises(S', nf' 
denotes the isotropic von Mises-Eisher distribution, with 
mean S G SO(3) and concentration parameter n. The key 
difference w.r.t. to measurement models in other PGO formu¬ 
lations lies in the use of the von Mises-Eisher distribution as 
the model for the rotational measurements errors R^ G SO(3). 

The isotropic von Mises-Fisher (or Langevin) (13 distri¬ 
bution on SO(n) with mean S G SO(n) and concentration 
parameter k>0 can be written explicitly as: 

P(^e) = ^^exp(Ktr (S''^Re)) , (2) 

c„(k) 

where tr(') is the matrix trace and Cn(/^) is a normalization 
term. Closed-form expressions for Cn{n) are given in (13; 
these are inconsequential for our derivation. Eor /i: ^ 0, the 
distribution tends to the uniform distribution over SO(n). Eor 
n oc, R^ = S with probability one. Roughly speaking, one 
may think at k, in terms of information content. 

We are now ready to introduce the maximum likelihood 
estimator for the poses, given the measurement model Q- 


B. Maximum Likelihood Estimator 

The ML estimate corresponds to the set of poses maxi¬ 
mizing the likelihood of the measurements, or, equivalently, 
minimizing the negative log-likelihood: 

/ml = , E - ^og C{Rij\x) -log C{iij\x). (3) 

{.=,eSE(3)} 

The negative log-likelihood of the Cartesian measurements can 
be easily computed from the Gaussian distribution: 

- log C{iij\x) = \\tj-ti-Riiij\f + const. (4) 
Using the negative log-likelihood for Rij is: 

— log C{Rij\x) = tr ifElj RiRij^ + const. 

= -^\\Rj - RiRij lip + const. 

where || • ||p is the Erobenius matrix norm (sum of the squares of 
the entries), and we used Ills' — i^||p=tr [{S — R){S — R)^). 
The norm Has' —i^||p is usually referred to as the chordal 
distance between two rotations S and R ifT^ . 

Plugging and 0 back into 0 we obtain our ML 
estimator: 

^To keep notation simple, we consider measurements with the same 
distribution. The extension to heterogeneous and is trivial. 
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= min 

{ti 

{RieSO(3)} 


E 

ihj)es 




II ^ ^ ^ I|2 

H — ||-Rj —-Ri-Rij lip 


(PGO) 

( 6 ) 


The main difference between and formulations in re¬ 
lated work is the use of the chordal distance (related work 
instead uses the geodesic distance ||Log || ). 

References O, |IT^ show that for small residual errors 
\ ||i^j - RiRijW^ ^ ||Log (RjjRjRj)\f, making the for¬ 
mulations equivalent from a practical standpoint. 

The advantage of the formulation •nil is that it has a 
quadratic objective function. This facilitates the derivation of 
the Lagrangian dual problem, as shown in the next section. 


III. Lagrangian Duality in 3D PGO 

The main goal of this paper is to provide tools to check if 
a candidate SLAM solution x is globally optimal. If we knew 
the optimal cost this would be easy: calling /ml(') the 
objective function of ([^, if fnL{x) = /^l then x is optimal. 
Unfortunately, unknown. Our contribution is to show 

that we can compute close proxies of using duality theory. 
To make the derivation easier, we first rewrite the problem as 
a quadratic problem with equality constraints (Section |III-A| ), 
and then derive the dual (Section [III-B| ). 


A. Quadratic Problem with Quadratic Equality Constrains 

In this section, we rewrite (|^ in order to (i) have vector 
variables (the rotations Ri are matrices), and (ii) formulate 
the constraints Ri G SO(3) as quadratic equality constraints. 

We define Vi G Mp as the vectorized version of Rf. 
Vi = [Ri^^ R\‘^^ where Rf^^ is the kth row of 

Ri. We use the shorthand Vi = rows{Ri) to obtain the 
vector representation of a 3 x 3 matrix Ri. Using this 
parametrization, each summand in the objective in ([^ becomes 
(using that ||i^||F= Ilil^IlF in the first expression): 

LO^ \\tj-ti-Rdijf + ^ \\Rj-RlRj\\l 

= ‘^tWtj-ti-TijTif+ ‘^\\rj-Qijrif (7) 

where Tij = I 3 (g) fT. e Qij = I 3 (g) Rjj G and 

(g) is the Kronecker product. 

We cannot choose arbitrary vectors Vi G but have to 
limit ourself to choices of Vi that produce meaningful rows of 
a rotation matrix Ri G SO(3). The rotation group SO(3) is 
defined as SO(3) = G R^^^ : RJR = l 3 ,det(i^) = 1}, 
which, written in terms of the rows of Ri, becomes: 


RjRi = I3 ^ = { I 

det{Ri) = 1 R^ X Rf '> = Rf ^ 


if u = V, 
if u ^ 


u^v = 
1,2,3 

( 8 ) 


estimation in 0(3) rather than SO(3) (i.e., resulting matrices 
can have determinant det(i^) = ±1). Then in Proposition]^ 
we show how to reconcile our verification techniques to work 
directly on the original PGO problem (|^. 

Using 0 and ^ and relaxing the determinant constraints, 
we rewrite the PGO problem (|^ as: 


min 

ihj)es 




-TijVi 


CC p II 

+ ^ k. 


Qij't^i 


subject to 


rjEuvri = 1 , 

t*i Euv't^i — O 5 


U = V 
U ^ V 


u,v = 1,2,3 
i = 1,... ,n 


(9) 


where E^v is a 9 x 9 selection matrix composed of 3 x 3 blocks 
that are zero everywhere except the 3x3 block in position 
{u,v), which is the identity matrix. The matrices Euv are 
built such that rjEuv^i = R^^\ hence the constraints 

in 0 correspond to the orthonormality constraints in ([ 8 ]). 

In order to write 0 in a more compact matrix notation, 
we define the vector x = [{[,..., fj, rj,..., G R^^^. 
Using this notation, 0 becomes: 


Y = ufin 

X 

subject to 


\\Ax\\^ 

X^ EiuvX = 1, U = V 
X^EiuvX = 0, U ^ V 


( 10 ) 

u,v = 1,2,3 
i = 1,... ,n 


where the matrices A and Eiuv are obtained by stacking 
the coefficient matrices in with suitable zero blocks for 
padding; and uj\ are included in the definition of A. 

Finally, since absolute poses are not observable from relative 
measurements, we fix a pose to be our reference frame. 
Without loss of generality we fix the pose of the first node 
to the identity pose {ti = O 3 and = I 3 , or, equivalently 
ri = rows(l 3 )). This process is usually called anchoring. 
Fixing the first pose modifies ( p^ as follows: 


Y = min 

X 

subject to 


\\Ax-bf 

X^ EiuvX = 1, U = V 
x^ EiuvX = 0, u V 


( 11 ) 

u,v = 1,2,3 
i = 1,... ,n — 1 


where x G is obtained by removing the first pose 

from X, A is obtained by removing from A the columns 
corresponding to the first pose, and b is the known right-hand- 
side arising from anchoring; Eiuv are the same as Eiuv but 
without the rows and columns corresponding to the first pose. 

We conclude this section by transforming O into an 
equivalent problem with homogeneous objective (i.e., without 
constant terms in the squared cost). For this purpose, we note 
that solving ( pTj ) is the same as solving: 


/* = min 
subject to 


\\Ax - byf 


(primal problem) 


^ ^iuv^ — I5 
^iuv^ — O5 
y^ = l 


u = V 1 u,v = 1,2,3 
u ^ V j i = 1,... ,n — 1 

( 12 ) 


where x is the cross product. In other words, the rows of a 
rotation matrix have to be orthonormal, and have to satisfy 
the right-hand rule. To derive the dual problem we relax the 
second condition (det(i^) = 1 ), which amounts to performing 


meaning that the two problems have the same optimal ob¬ 
jective, and the corresponding solutions can be mapped to 
each other. Intuitively, if the solution of is [x^fj 1 ], then 
cc* = x'^fj is also optimal for 0 , while if the solution 
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is [x'^ — 1 ], then will be optimal for (TT\ . 

The inclusion of the slack variable y is often referred to as 
homogenization. We refer to as the primal problem. 

B. The dual problem 

In this section we apply Lagrangian duality to the primal 
problem ( p^ , borrowing the corresponding theory from the 
optimization community 03, 1^ . We begin by recalling 
basic properties and notions about duality theory and then we 
tailor these concepts to our SLAM problem. 

The key insight of duality is that for every constrained 
optimization problem of the form: 


where the first inequality stems from the fact that A* attains the 
maximum over all A, and the last follows from the optimality 
of /* (which is the global minimum among all feasible x). 

Therefore, in this work we exploit a simple idea: given a 
candidate solution x, if we are able to find a A, for which 
d(A) = /(£), then the chain of inequalities ( p^ becomes 
tight {d{X) = d^ = /* = f{x)), which implies that x is 
an optimal solution. Equation ( p^ thus provides a means of 
certifying the global optimality of a candidate solution x for 
( p^ and enables our derivation of algorithmic approaches for 
certifying the correctness of SLAM solutions. 

We are now ready to apply duality to our primal prob¬ 
lem ( p^ . From ( p^ and ( p^ , the dual function is: 


/* = min^, f{x) (13) 

subject to Ci{x) =0 G C 

(where C is a set indexing the constraints q(x)), there is an 
associated unconstrained optimization problem: 

_ dW _ 

d^ = max ( inf f{x) + ^ XiCi{x) j (14) 

called the dual problem. The scalar variables appearing in 
( p^ are called Lagrange multipliers or dual variables, and the 
function d(A) is called the dual function', A is a vector stacking 
all dual variables. With reference to the dual problem ( p^ , 
problem ( p3] ) is referred to as the primal problem. Intuitively, 
the minimization (“inf”) in d{X) can be understood as a 
relaxation of the original problem ( p^ in which the constraints 
are transformed into penalty terms in the objective, whose 
“importance” is controlled by A; hence, the maximization 
(w.r.t.. A) tries to make this relaxation as tight as possible. 

The dual problem ([T^ has two important properties. First, 
since the dual function d{X) is the pointwise infimum of a 
family of affine functions of A, it is always concave, and 
therefore the dual maximization problem ([T§ is a convex 
program ns Sec. 5.2]. Its convexity guarantees that the dual 
problem can always be solved globally optimally using local 
search techniques. Second, given any feasible x for ( p^ (for 
which Ci{x) = 0), the definition of d{X) in ( p^ shows that 
d{X) < f{x) for any choice of A. In particular, this must also 
hold at the optima x^ and A* in ( p3] ) and ( p^ , so that: 

< r. (15) 

The inequality is referred to as weak {Lagrangian) duality, 
and it enables us to lower-bound the optimal value /* of the 
(possibly very difficult, nonconvex) primal problem ( p^ using 
the optimal value d^ of the (convex) dual problem (T^ . For 
some problems, the inequality ( p3] ) is tight (i.e. d* = /*), 
for which we say that strong {Lagrangian) duality holds. The 
quantity /* — d* > 0 is called the duality gap. 

Using weak duality, it is easy to show that, given a primal 
feasible point x and a dual point A, the following chain of 
inequality holds: 

d{X) < d{X*) =d*<r = fix*) < fix) (16) 


n—1 r 


diX) = inf \\Ax - by\f + Y] Ai„„(l - x~^EiuuX) 

^iV 


i=l ^U=l,2,3 


^ ^ ^iuv{ ^ 


’i,v=l,2,3 

u^v 


+ Ay(i - y^), 


(17) 


x-\yy^ 


where A is the vector of Lagrange multipliers and Ay, as¬ 
sociated with the orthonormality and homogeneity constraints 
in respectively. We observe that the quadratic terms 

in GD can be written more compactly as: 

rn-l 3 

\\Ax - by\\^ -x'^ ™ 

i=l u,v=l 


(18) 


where 

n—1 3 

if (A) = A'^A - Y.^iuvEiuv. (19) 

i=l U,V = 1 

Calling M{X) the matrix in ( p^ , the dual function (Tf) can 
thus be written as: 

T 


X 

T 

if(A) -A'^b 

X 

. y . 


-b^A b^b - Xy 

. y . 


d{X) = inf 

x,y 


X 

y 


M{X) 


X 

y 


Kuu + Ay (20) 


w=l,2,3 


Now in the dual problem ( p^ we try to maximize d(A); 
however, from ( [^ we see that d{X) = —oo if M{X) has a 
negative eigenvalue (by letting [x^y] lie in the corresponding 
eigenspace). Consequently, we can safely restrict our search 
to the vectors A that preserve positive semi-definiteness of 
M{X) (191 Sec. 5.1.5]. Moreover: 

T 


M(A) ^ 0 


inf 


X 

y 


M{X) 


X 

y 


= 0 , ( 21 ) 


as the minimization over [x y] in the homogenized problem 
is unconstrained. The dual problem ( p^ thus becomes: 

d^ = maxA Xiuu + \ (dual problem) 

u=l,2,3 

subject to M{X) y 0. ( 22 ) 


The dual SLAM problem turns out to be a semidefinite 
program (SDP), for which specialized solvers exist (211 . In the 
following section we discuss the relations between the primal 
and the dual problem, and elucidate on its practical use. 
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IV. Relation between the Primal and 
THE Dual Problem and Practical Use 

In this section we present two powerful applications of the 
dual problem ( [22| ). Section [TV-AI deals with the case in which 
one is given a candidate PGO solution, and wants to evaluate 
its quality, possibly certifying its optimality. Section IV-B| 
shows that in particular cases (when the duality gap is zero) 
one can obtain an optimal solution of the primal problem from 
the solution A* of the dual. 

In both sections, we use the following property. 

Lemma 1 (Primal optimal solution and zero duality gap): 
If the duality gap is zero (d* = /"*"), then any primal optimal 
solution [cc* 1 ] of ( p^ is in the null space of the matrix 
where A"*" is the solution of the dual problem ( [ 22 | ). 

Proof: When the duality gap is zero, any minimizer of 
the primal problem is also a minimizer for the infimum in the 
dual function ( p^ iP^ Sec. 5.5.5]. Consider a primal optimal 
solution [x^ 1]. We already observed than any such minimizer 
annihilates the quadratic term in ( | 20 | ), and therefore it holds 
that [cc* 1]^ 1 ] = 0 , which implies that [cc* 1 ] is in 
the null space of proving the claim. ■ 

We give an alternative proof, which does not require prior 
knowledge on duality, in Appendix A. 


A. Verification 

In this section we consider the case in which we are given 
a candidate solution x for the primal problem, and we want 
to evaluate the quality of this solution. For brevity, we denote 
with f{x) the objective evaluated at x, for both O and its 
homogeneous form (for the latter we imply y = 1). 

We begin with the following proposition, whose proof easily 
follows from ( p^ and the discussion in Section |III-B| 
Proposition 2 (Verification of Primal Optimal Objective): 
Given a candidate solution x for the primal problem ( p^ , if 
f{x) = dA, then the duality gap is zero and x is an optimal 
solution of Moreover, even if the duality gap is nonzero, 
f{x) — d^ > f{x) — f'^, meaning that f{x) — d^ is an 
upper-bound for the sub-optimality gap of x. 

Proposition!^ ensures that the candidate x is optimal when 
f(x) = dA. Moreover, even in the case in which we get 
f{x) > d*, the quantity f(x) —dA can be used as an indicator 
of how far x is from the global optimum. 

While Proposition!^ already provides means of verifying a 
candidate solution, it requires solving the dual problem, to 
compute dA. The following proposition provides a technique 
to verify the optimality of x without solving the SDR 
Proposition 3 (Verification of Primal Optimal Solution): 


the solution A of the linear system 

H{X*) ] 


A^b 




b^A 

X = 

. K - . 

M(A) 

X 

1 

= 0 (to be solved w.r.t. A) (23) 





is such that M(A) ^ 0 and d{X) = f{x), then the duality 
gap is zero and x is a primal optimal solution. 

Proof: From Lemma we know that when the duality 
gap is zero, it must hold M{X^)[x'^ 1] = 0. Therefore, 


in Proposition!^ we solve the linear system ( |^ , trying to ob¬ 
tain A*. When M(A) ^ 0, the solution A of is such that 
d{X) < dd (recall that dd is the maximum over A). Therefore, 
it holds that (i) d(X) < d* < /* < f{x) (by weak duality 
and optimality of /*). However, if d{X) = f{x), the chain 
of inequalities (i) becomes tight, d{X) = dd = f^ = 
implying that x attains the optimal objective /*. ■ 

In practice, iterative SLAM solvers optimize (!^, rather than 
the primal problem A natural question is then how to 
use the results in Propositions !2]!^ (which relate /* and d*) to 
verify the solution of whose optimal value is /jJl > /*• 
This extension is given by the following proposition, which 
essentially states that Propositions can be applied directly 
to check the solution of 

Proposition 4 (Verification techniques for PGO): The fol¬ 
lowing statements hold true: 

(Vl) Given a candidate solution x for the PGO problem (!^, 
if fnhix) = d^, then x is an optimal solution of (!6). 
Moreover, /ml(®) - d * > /ml(®) - /ml> /ml(®) - d * 
is an upper-bound for the sub-optimality gap of x. 

(V2) Given a candidate solution x for the PGO problem (!^, 
if the solution A of the linear system ^23^ is such that 
M(A) ^ 0 and d(X) = then the duality gap is 

zero and x is an optimal solution of 
Proof: The first claim can be proven by observing that 
the following chain of inequalities holds (i) dd < f^ < 
fnL < hhix), hence fnL{x) = d^ implies that d^ = f'^ = 
/ml = which implies that x is optimal. The inequality 

fnL{x) — d'^ > —/ml easily follows from (i). The second 

claim can be proven in the same way, noting that (ii) d{X) < 
d^<r< fk < if we are able to compute a A that 

is dual feasible (M{X) ^ 0) and such that d{X) = 
then it must hold d{X) = dd = f^ = /jJl = fv[L{dd), which 
implies that /ml(^^) is optimal. ■ 


B. Primal Optimal Solutions 

In this section, rather than verifying the quality of a given 
candidate solution, we show how to use duality to compute 
a primal optimal solution directly. We focus on the particular 
case in which the duality gap is zero. This case is of interest 
as we observe that the duality gap is often zero in practice. 

From Lemma we know that an optimal solution must 
be in the null space of AT (A*). This motivates the following 
proposition, which provides a way to compute a primal optimal 
solution directly from the solution A"*" of ( ! 22 | ). 

Proposition 5: If the duality gap is zero and A* is an 
optimal solution of ^22) , then an optimal solution x^ of Gl 
can be computed by solving the following linear system: 


(to be solved w.r.t. x^) 
(24) 


gap is zero, it holds M{X'^)[x'^ 1] = 0. Then, recalling the 
structure of M{X^) from ( [T^ , it’s easy to see that ( !24| ) only 
rewrites the condition M{X'^)[x'^ 1 ] = 0 , moving the constant 
terms to the right-hand side. ■ 
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Fig. 2. Statistics regarding the verification technique V1 (first row), the computation of the primal solution via the dual (second row), and the verification 
technique V2 (third row). Results are shown for different levels of translation noise ctt (first column), rotation noise cfr (second column), and size of the 
problem (third column). The rightmost column shows the CPU time required by V1 and V2 and the datasets used for the Monte Carlo simulations. 


Note that Lemma ensures that the linear system ( [24| ) 
admits a solution. Proposition allows finding an optimal 
solution to the primal problem ( p^ . If this satisfies the 
determinant constraints in 0, it follows that this solution is 
also optimal for the original PGO problem ([^. While currently 
we cannot prove that the determinant constraints are always 
satisfied, this was always the case in our experiments. 

V. Experiments 

In this section we show that the first verification technique in 
Proposition]^ (referred to as V1) enables accurate quantification 
of the sub-optimality gap of a candidate SLAM solution, but 
that current SDP solvers do not scale well with increasing 
problem size. The second verification technique in Proposi¬ 
tion]^ (referred to as V2) provides a convenient alternative for 
large-scale problems: it is reliable and less computationally 
demanding. Finally, we provide empirical evidence that when 
the duality gap is zero we can compute an optimal solution 
from the solution of the dual problem, as suggested by 
Proposition]^ The SDP ( ]22| ) is solved using SDPA tSTIl . 

Effectiveness of V1. PropositionQvi) ensures that for any 
candidate solution x, /ml(^^) — represents an upper-bound 
on the sub-optimality gap fm.{db) — /ml However, it is 

possible that this bound is very loose, in which case it would 
be of little practical utility. In this section, we show that 
is close to practice, hence — d* is a very good 

measure of the sub-optimality gap of the candidate x. 

In our experiments we compute the “optimal” solution 
of (]^ by refining the chordal initialization of 12 ^ . (2^ with 10 
Gauss-Newton (GN) iterations. While one cannot guarantee a 


priori that this approach always produces the optimal estimate, 
using the results of this paper we will be able to check 
optimality a posteriori. 

We evaluated how close d^ is to on the cube dataset of 
Fig.]^b4). In this dataset, the odometric trajectory is simulated 
as the robot travels on a 3D grid world, and random loop 
closures are added between nearby nodes, with probability 
0.3. Relative pose measurements are obtained by contaminat¬ 
ing the true relative poses with zero-mean Gaussian noise, 
with standard deviation ctt and cfr for the translational and 
rotational noise, respectively. Statistics are computed over 10 
runs: for each run we create a cube with random connectivity 
and random measurement noise. We consider an example with 
n = 5^ poses and varying noise levels cft and cfr. 

Fig.]^al) shows d^ and for different translational noise 
levels, fixing cfr = O.OSrad. The figure shows that — /ml 
(zero duality gap) independently on the translational noise 
level, hence d^ is a very good proxy of 

Fig. ]^a2) shows d^ and for different rotational noise, 
fixing (Jt = 0.1m. In this case the duality gap f^i^ — d^ is more 
sensitive to the noise level, and for large rotational noise d^ 
becomes smaller than However, the gap /jJ^ — d* remains 
small, and is within 20% of /]Jl in all cases. 

Fig. ]^a3) shows d^ and /]Jl for different sizes of the cube 
dataset, fixing aR = O.OSrad and (Tr = 0.1m. Again in this 
case d* = (zero duality gap) independently of the size of 
the dataset. However, we observe that the SDP ( ]22| ) becomes 
intractable for larger problem sizes, as shown in Fig. ]2a4). 

Primal optimal solution via the dual. Here we demonstrate 
experimentally that one can recover a primal optimal solution 
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for the SLAM problem from the dual optimal solution A* 
of whenever strong duality holds. 

For each of the experimental trials described previously, we 
also computed an estimate x for the SLAM solution directly 
from the optimal solution A* for the dual program ( |22| ) using 
equation ( [24| ). Figs. [^bl)-(b3) compare the value f{x) of 
this estimate against the value of the solution obtained 
by solving the SLAM problem directly using the chordal 
initialization, and against the cost of the odometric guess. 

We can see that for those experimental conditions in which 
strong duality holds (those experiments in Figs. [^al)-(a3) for 
which the green and red bars are the same height), the estimate 
X in fact achieves the (certified) globally optimal cost and 
is therefore a globally optimal solution for the SLAM problem, 
as guaranteed by Proposition More interestingly, we also 
find that even in those cases when strong duality does not 
hold (in which case Proposition no longer guarantees that 
X is a primal optimal solution), the quality of the candidate 
X degrades gracefully (i.e. the gap f{x) — increases 
gradually) with increasing noise levels. In particular, we find 
that X outperforms the odometric guess in all tested cases. 

These experiments confirm that we can extract a primal 
optimal solution x^ for the SLAM problem directly from 
the solution A* of the convex dual problem whenever strong 
duality holds. However, this approach is unfortunately not 
currently practical as a general-purpose SLAM technique, due 
to the high computational cost of solving large scale SDPs. 

Effectiveness of V2. In this section we show that V2 
is a computationally tractable verification approach, and it 
preserves the desirable properties of V1, i.e., it is able to 
discern optimal estimates from suboptimal ones. In contrast 
to V1, V2 does not quantify the sub-optimality gap, but can 
only give a binary answer: either it certifies that the estimate 
X is optimal (by producing a dual certificate A), or it is 
inconclusive. Before moving to the real tests, we consider the 
cube scenario discussed in the previous section. 

We perform the following test: for each realization of the 
cube scenario, we compute the optimal solution x^ (attaining 
/j^l) in the previous tests, and a sub-optimal solution 
x^ (attaining > /*) by bootstrapping the Gauss-Newton 
method with a random initial guess. Then, we apply the 
verification technique to both x^ and and see if they pass 
the optimality test. Using the results we can compute the 
precision and recall of our classification: 


precision = 


|/ cc*|-h|/ x'^\ 


recall = 


1/ ®*|+|x®*| 


(25) 


where | / x'^\ denotes the number of tests in which an optimal 
solution x^ was accepted as optimal by V2, |/ is the 
number of tests in which a suboptimal solution was accepted 
as optimal, and |X is the number of tests in which V2 was 
not able to certify the optimality of an optimal solution. 

We point out that Proposition[^V2) guarantees that our cer¬ 
tification approach always has precision equal to 1; however, 
recall may be less than 1, and will be for cases in which strong 
duality does not hold. We plot precision/recall for different 
levels of translational and rotational noise, and for different 
sizes of the dataset, as shown in Figs. |^cl)-(c2)-(c3). We 





d(A) 


Time 

V2 


sphere 

Init. 

5.7595-10^ 

5.75-102 

-1.80-10“^ 

609 

/ 

n = 2500 
m = 4949 

Odom. 

5.8019-102 

4.38-102 

-1.45-10“^ 

597 

X 

sphere-a 

Init. 

1.2485-10^ 

1.25-10® 

-9.64-10“® 

332 

/ 

n = 2200 
rn. = 8647 

Odom. 

3.0413-10® 

3.04-10® 

-1.01-102 

2.5 

X 

torus 

Init. 

1.2114-10^ 

1.21-10^ 

-7.85-10“2 

39 

/ 

n = 5000 
m = 9048 

Odom. 

2.7666-10^ 

2.76-10^ 

-1.04-102 

3.7 

X 

cube 

Init. 

4.216-10^ 

4.22-10^ 

-1.24-10“2 

1646 

/ 

n = 8000 
m = 22236 

Odom. 

2.7465-10® 

2.74-10® 

-9.98-10^ 

68.2 

X 

garage 

Init. 

6.2994-10“^ 

6.11-10“^ 

-6.78-10“2 

12.3 

7 

n=1661 
m = 6275 

Odom. 

6.2997-10“^ 

3.53-10“^ 

-6.75-10“2 

11.2 

X 

cubicle 

Init. 

6.2481-102 

6.25-102 

-4.76-10“^ 

16.8 

/ 

n, = 5750 
m = 16869 

Odom. 

6.2484-102 

6.16-102 

-4.75-10“^ 

15.5 

/ 

rim 

Init. 

1.235-10^ 

1.23-10^ 

-9.77-10^ 

7.7 

X 

n=10195 
m = 29743 

Odom. 

1.6985-10^ 

-2.8-10^ 

-9.38-10^ 

7.1 

X 


TABLE I 

Verification technique V2 on large-scale SLAM datasets. 


observe that only for larger rotational noise Fig. |^c2) the 
recall decreases; these are exactly the cases in which the 
duality gap is nonzero. (For (7^ = 0.2rad, V2 was not able to 
certify optimality in any case, which means that the precision 
becomes undefined (^) and for this reason we do not show 
the corresponding data point in Fig.[^c2).) Finally, Fig.[^d2) 
shows the CPU time required by V2. This is the time required 
to solve the linear system ( |^ , and check if AT (A) ^ 0. V2 is 
computationally cheap, as it does not require solving the SDP. 

We conclude the experimental part of this paper by testing 
the performance of the verification technique V2 on large-scale 
SLAM datasets. We consider the same datasets as 1 ^ : the 
sphere, sphere-a, torus and cube are simulated datasets, while 
the garage, cubicle and rim are real datasets. For the results in 
this paper we substituted the covariances in the datasets of the 
scenarios sphere, sphere-a, garage, cubicle and rim with isotropic 
ones, as required by the PGO formulation ([^. 

Table |T| shows the results of the application of V2 to the 
SLAM datasets. The first column shows the cost obtained by 
applying a GN method starting from the initialization 1221 
(rows “Init.” in the table) and from the odometric guess 
(rows “Odom.”). These are the candidate solutions that we 
want to check, using V2. The columns “d(A)” and “/i” show 
intermediate results of V2. In particular, d{X) is the same 
one described in Proposition]^ V2), while p is the smallest 
eigenvalue of AT (A). Recall that V2 certifies optimality when 
fnhi^) = dW and /i > 0 (if the smallest eigenvalue 
is non-negative, then AT(A) ^ 0). We specify a tolerance 
in these tests, since the GN estimate x will not attain the 
optimal solution x^ exactly. Consequently, the solution A 
of ^23^ is not exact, so we consider that /ml(^^) = d{X) 
if \fKL{x) — d{X)\/fi/[L{x) < 20%. Similarly, we add some 
tolerance to the condition /i>0, and accept /i>—1. Recall that 
/i is expected to be slightly negative, as the objective in ( [22| ) 
tends to push AT(A) towards the boundary of the positive 
definite cone, and the SDP is solved numerically. 

Let us start our analysis from the sphere dataset. When 
using the initialization (“Init.” row), the GN method attains 
an objective 5.7595 • 10^. In this case, the two conditions 
for V2 are satisfied and we can certify the optimality of the 
resulting estimate (green check-mark in the rightmost column). 
When using the odometric initialization the resulting cost is 
larger (red entry in the column /ml(^^)), hence the estimate 
is suboptimal. V2 is able to identify the suboptimality, since 



















d{X) becomes much smaller than Hence V2 correctly 

decides not to certify the optimality of the odometric estimate 
(red “X” in the last column). Similar considerations hold for 
the second scenario, the challenging sphere-a: in this case the 
initialized estimate (shown in Fig. top left) is accepted 
as optimal. The odometric estimate, instead, is trapped in a 
local minimum (Fig.[2 bottom left), and the optimality test V2 
correctly rejects the estimate since it leads to a very negative /i. 
Similar considerations hold for the torus, cube, and the cubicle 
datasets: our technique is able to discern optimal solutions 
from suboptimal estimates in all cases. 

For the garage dataset, the initialized estimate is classified 
as optimal, while the odometric estimate, which has a similar 
cost (6.2997-10“^ vs 6.2994-10“^), is rejected as suboptimal. 
We observed the corresponding trajectory estimates (see Fig|^ 
and, while they are are both visually correct and have very 
similar costs, they do not overlap. This may indicate the pres¬ 
ence of regions in the cost functions that are nearly fiat, i.e., 
for which different estimates can have similar cost. Allowing 
for extra GN iterations, the odometric estimate converges to 
the same cost as the initialized one, and our technique is 
able to certify its optimality. Empirically, we observed that 
estimates that are suboptimal because they need extra iterations 
to converge tend to fail the check on d{X) (compare with 
sphere and garage), while estimates that converge to a wrong 
minimum tend to fail the check on /i (compare with sphere-a, 
torus, and cube). 

For the rim dataset, both estimates (“init” and “odom”) 
are rejected by V2. While the initialized estimate is visually 
correct, currently, we cannot conclude anything as it might 
be that there exists a better estimate (i.e., GN failed), or our 
technique failed because the duality gap was nonzero. 

Finally, in the column “Time”, we report the CPU time (in 
seconds) required to perform the second verification technique. 
Most of the time here is spent computing the smallest eigen¬ 
value fi of M{X), which was obtained using Matlab’s eigs, 
specifying —100 as guess for the eigenvalue. The CPU time 
depends on the size of the problem, but also depends on the 
distance between our guess (—100) and the closest eigenvalue. 
We leave a more thorough investigation of these computational 
aspects for future work. 

VI. Conclusion 

We show that Lagrangian duality is an effective tool to 
assess the quality of a given SLAM estimate. We propose two 
techniques to judge if an estimate is globally optimal (i.e., it 
is the ML estimate), and we show that when the duality gap 
is zero one can compute an optimal SLAM solution from the 
dual problem. The performance of our verification techniques 
is extensively tested in real and simulated datasets, including 
large-scale benchmarks. Many theoretical questions remain 
open, e.g., under which conditions the duality gap is zero? 
is it possible to derive bounds on the duality gap? We also 
leave as future work more practical problems, such as the one 
of exploring more efficient solvers for large SDPs, possibly 
exploiting problem structure. 
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Appendix 

A. Extended Proof of Lemma [7] 

Here we give an alternative proof of Lemma For the sake 
of readability, we restate the Lemma before proving it. 

Lemma 6 (Primal optimal solution and zero duality gap): 
If the duality gap is zero (d* = /"*"), then any primal optimal 
solution [cc* 1] of ( p^ is in the null space of the matrix 
where A* is the solution of the dual problem ( |22| ). 

Proof: Assume that the duality gap between the primal 
problem (V2) and the dual problem ( |22| ) is zero: 


d* = f* ^ + X*y = \\Ax* - by 


,*l|2 


(26) 


w=l,2,3 


Now, since [cc* y'^] is a solution of the primal problem, it must 
also be feasible, hence the following equality holds: 


^ „„(**)) (27) 

= 1,2,3 


E n —1 

i=l 


i,7;=l,2,3 

u^v 


+ A*(l-(y*)^)=0 


Therefore, we can subtract the left-hand side of to the 
left-hand side of ([26|) without altering the result: 


n—1 r 


-E E „„(**)) 


2=1 >-^= 12,3 




i,v=l,2,3 

u^v 




E A*„„ + A; = ||Aa;*-6y*f (28) 


i=l,...,n 

w=l,2,3 


Noting that some terms in and A* cancel out, we get: 


n—1 r 


E E E Ei^x*) 

2=1 1 -^= 1 , 2,3 


u,v=l,2,3 

u^v 


= \\Ax^ -hy'^W" 

Reorganizing the terms as done in ( p^ , we obtain: 

= 0 


X* 

T 

H{\*) -A^b 

■ ■ 

y* 


-b^A b-^b-x; 

. y* 


(29) 


which implies that [cc* y'^] is the null space of M (A*), proving 
the claim. ■ 


B. Estimated Trajectories for the Datasets in Table 


GN from initialization GN from odometrv 
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Fig. 3. Blue: Estimated trajectory obtained by refining the chordal initial¬ 
ization of Ea with 10 Gauss-Newton iterations. Red: Estimated trajectory 
obtained by refining the odometric guess with 10 Gauss-Newton iterations. 
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Fig. 4. garage dataset: while the estimates obtained from GN with ini¬ 
tialization (blue) and GN from odometry (red) attain a very similar cost 
(6.2994-10“^ versus 6.2997-10“^), the corresponding trajectories are still 
different, which indicates that GN from odometry did not completely converge 
after 10 iterations. 








